/*
 * Copyright (c) Thorben Linneweber and others
 *
 * Permission is hereby granted, free of charge, to any person obtaining
 * a copy of this software and associated documentation files (the
 * "Software"), to deal in the Software without restriction, including
 * without limitation the rights to use, copy, modify, merge, publish,
 * distribute, sublicense, and/or sell copies of the Software, and to
 * permit persons to whom the Software is furnished to do so, subject to
 * the following conditions:
 *
 * The above copyright notice and this permission notice shall be
 * included in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
 * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
 * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
 * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 */

using System.Diagnostics;
using System.Runtime.CompilerServices;
using Jitter2.LinearMath;

using Vertex = Jitter2.Collision.MinkowskiDifference.Vertex;

#pragma warning disable CS0649 // Field is never assigned to, and will always have its default value

using SoftFloat;

namespace Jitter2.Collision
{
	
	// This is a GJK-Implementation "by the book".
	
	public unsafe struct SimplexSolverAB
	{
	    sfloat Epsilon = 1e-8f;
	
	    private struct Barycentric
	    {
	        public sfloat Lambda0;
	        public sfloat Lambda1;
	        public sfloat Lambda2;
	        public sfloat Lambda3;
	
	        public sfloat this[int i]
	        {
	            get => ((sfloat*)Unsafe.AsPointer(ref this.Lambda0))[i];
	            set => ((sfloat*)Unsafe.AsPointer(ref this.Lambda0))[i] = value;
	        }
	    }
	
	    private Vertex v0;
	    private Vertex v1;
	    private Vertex v2;
	    private Vertex v3;
	
	    private Barycentric barycentric;
	    private uint usageMask;
	
		public SimplexSolverAB()
        {
        }

	    public void Reset()
	    {
	        usageMask = 0;
	    }
	
	    private NVector3 ClosestSegment(int i0, int i1, ref Barycentric bc, out uint mask)
	    {
	        var ptr = (Vertex*)Unsafe.AsPointer(ref this.v0);
	        NVector3 a = ptr[i0].V;
	        NVector3 b = ptr[i1].V;
	
	        NVector3 v = b - a;
	        sfloat vsq = v.LengthSquared();
	
	        bool degenerate = vsq < Epsilon;
	
	        sfloat t = -NVector3.Dot(a, v) / vsq;
	        sfloat lambda0 = 1 - t;
	        sfloat lambda1 = t;
	
	        mask = (1u << i0 | 1u << i1);
	
	        if (lambda0 < 0 || degenerate)
	        {
	            mask = 1u << i1;
	            lambda0 = 0;
	            lambda1 = 1;
	        }
	        else if (lambda1 < 0)
	        {
	            mask = 1u << i0;
	            lambda0 = 1;
	            lambda1 = 0;
	        }
	
	        bc[i0] = lambda0;
	        bc[i1] = lambda1;
	
	        return lambda0 * a + lambda1 * b;
	    }
	
	    private NVector3 ClosestTriangle(int i0, int i1, int i2, ref Barycentric bc, out uint mask)
	    {
	        mask = 0;
	
	        var ptr = (Vertex*)Unsafe.AsPointer(ref this.v0);
	        NVector3 a = ptr[i0].V;
	        NVector3 b = ptr[i1].V;
	        NVector3 c = ptr[i2].V;
	
	        NVector3.Subtract(a, b, out var u);
	        NVector3.Subtract(a, c, out var v);
	
	        NVector3 normal = u % v;
	
	        sfloat t = normal.LengthSquared();
	        sfloat it = (sfloat)1.0f / t;
	
	        bool degenerate = t < Epsilon;
	
	        NVector3.Cross(u, a, out var c1);
	        NVector3.Cross(a, v, out var c2);
	
	        sfloat lambda2 = NVector3.Dot(c1, normal) * it;
	        sfloat lambda1 = NVector3.Dot(c2, normal) * it;
	        sfloat lambda0 = (sfloat)1.0f - lambda2 - lambda1;
	
	        sfloat bestDistance = sfloat.MaxValue;
	        UnsafeEx.SkipInit(out NVector3 closestPt);
	        UnsafeEx.SkipInit(out Barycentric b0);
	
	        if (lambda0 < (sfloat)0.0f || degenerate)
	        {
	            var closest = ClosestSegment(i1, i2, ref b0, out uint m);
	            sfloat dist = closest.LengthSquared();
	            if (dist < bestDistance)
	            {
	                bc = b0;
	                mask = m;
	                bestDistance = dist;
	                closestPt = closest;
	            }
	        }
	
	        if (lambda1 < (sfloat)0.0f || degenerate)
	        {
	            var closest = ClosestSegment(i0, i2, ref b0, out uint m);
	            sfloat dist = closest.LengthSquared();
	            if (dist < bestDistance)
	            {
	                bc = b0;
	                mask = m;
	                bestDistance = dist;
	                closestPt = closest;
	            }
	        }
	
	        if (lambda2 < (sfloat)0.0f || degenerate)
	        {
	            var closest = ClosestSegment(i0, i1, ref b0, out uint m);
	            sfloat dist = closest.LengthSquared();
	            if (dist < bestDistance)
	            {
	                bc = b0;
	                mask = m;
	                closestPt = closest;
	            }
	        }
	
	        if (mask != 0) return closestPt;
	
	        bc[i0] = lambda0;
	        bc[i1] = lambda1;
	        bc[i2] = lambda2;
	
	        mask = (1u << i0) | (1u << i1) | (1u << i2);
	        return lambda0 * a + lambda1 * b + lambda2 * c;
	    }
	
	    private NVector3 ClosestTetrahedron(ref Barycentric bc, out uint mask)
	    {
	        [MethodImpl(MethodImplOptions.AggressiveInlining)]
	        sfloat Determinant(in NVector3 a, in NVector3 b, in NVector3 c, in NVector3 d)
	        {
	            return NVector3.Dot(b - a, NVector3.Cross(c - a, d - a));
	        }
	
	        sfloat detT = Determinant(v0.V, v1.V, v2.V, v3.V);
	        sfloat idetT = (sfloat)1.0f / detT;
	
	        bool degenerate = detT * detT < Epsilon;
	
	        sfloat lambda0 = Determinant(NVector3.Zero, v1.V, v2.V, v3.V) * idetT;
	        sfloat lambda1 = Determinant(v0.V, NVector3.Zero, v2.V, v3.V) * idetT;
	        sfloat lambda2 = Determinant(v0.V, v1.V, NVector3.Zero, v3.V) * idetT;
	        sfloat lambda3 = (sfloat)1.0f - lambda0 - lambda1 - lambda2;
	
	        sfloat bestDistance = sfloat.MaxValue;
	
	        UnsafeEx.SkipInit(out NVector3 closestPt);
	        UnsafeEx.SkipInit(out Barycentric b0);
	
	        mask = 0;
	
	        if (lambda0 < (sfloat)0.0f || degenerate)
	        {
	            var closest = ClosestTriangle(1, 2, 3, ref b0, out uint m);
	            sfloat dist = closest.LengthSquared();
	            if (dist < bestDistance)
	            {
	                bc = b0;
	                mask = m;
	                bestDistance = dist;
	                closestPt = closest;
	            }
	        }
	
	        if (lambda1 < (sfloat)0.0f || degenerate)
	        {
	            var closest = ClosestTriangle(0, 2, 3, ref b0, out uint m);
	            sfloat dist = closest.LengthSquared();
	            if (dist < bestDistance)
	            {
	                bc = b0;
	                mask = m;
	                bestDistance = dist;
	                closestPt = closest;
	            }
	        }
	
	        if (lambda2 < (sfloat)0.0f || degenerate)
	        {
	            var closest = ClosestTriangle(0, 1, 3, ref b0, out uint m);
	            sfloat dist = closest.LengthSquared();
	            if (dist < bestDistance)
	            {
	                bc = b0;
	                mask = m;
	                bestDistance = dist;
	                closestPt = closest;
	            }
	        }
	
	        if (lambda3 < (sfloat)0.0f || degenerate)
	        {
	            var closest = ClosestTriangle(0, 1, 2, ref b0, out uint m);
	            sfloat dist = closest.LengthSquared();
	            if (dist < bestDistance)
	            {
	                bc = b0;
	                mask = m;
	                bestDistance = dist;
	                closestPt = closest;
	            }
	        }
	
	        if (mask != 0) return closestPt;
	
	        barycentric[0] = lambda0;
	        barycentric[1] = lambda1;
	        barycentric[2] = lambda2;
	        barycentric[3] = lambda3;
	
	        mask = 0b1111;
	        return NVector3.Zero;
	    }
	
	    public void GetClosest(out NVector3 pointA, out NVector3 pointB)
	    {
	        pointA = NVector3.Zero;
	        pointB = NVector3.Zero;
	
	        var ptr = (Vertex*)Unsafe.AsPointer(ref this.v0);
	
	        for (int i = 0; i < 4; i++)
	        {
	            if ((usageMask & (1u << i)) == 0) continue;
	            pointA += barycentric[i] * ptr[i].A;
	            pointB += barycentric[i] * ptr[i].B;
	        }
	    }
	
	    public bool AddVertex(in NVector3 vertex, out NVector3 closest)
	    {
	        UnsafeEx.SkipInit(out Vertex fullVertex);
	        fullVertex.V = vertex;
	        return AddVertex(fullVertex, out closest);
	    }
	
	    public bool AddVertex(in Vertex vertex, out NVector3 closest)
	    {
	        UnsafeEx.SkipInit(out closest);
	
	        var ptr = (Vertex*)Unsafe.AsPointer(ref this.v0);
	        int* ix = stackalloc int[4];
	
	        int useCount = 0;
	        int freeSlot = 0;
	
	        for (int i = 0; i < 4; i++)
	        {
	            if ((usageMask & (1u << i)) != 0) ix[useCount++] = i;
	            else freeSlot = i;
	        }
	
	        ix[useCount++] = freeSlot;
	        ptr[freeSlot] = vertex;
	
	        switch (useCount)
	        {
	            case 1:
	            {
	                int i0 = ix[0];
	                closest = ptr[i0].V;
	                usageMask = 1u << i0;
	                barycentric[i0] = (sfloat)1.0f;
	                return true;
	            }
	            case 2:
	            {
	                int i0 = ix[0], i1 = ix[1];
	                closest = ClosestSegment(i0, i1, ref barycentric, out usageMask);
	                return true;
	            }
	            case 3:
	            {
	                int i0 = ix[0], i1 = ix[1], i2 = ix[2];
	                closest = ClosestTriangle(i0, i1, i2, ref barycentric, out usageMask);
	                return true;
	            }
	            case 4:
	            {
	                closest = ClosestTetrahedron(ref barycentric, out usageMask);
	                return usageMask != 0b1111;
	            }
	        }
	
	        Debug.Assert(false, "Unreachable.");
	        return false;
	    }
	}
}
